// PCL includes
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/io/pcd_io.h"
	#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnCppWrapper.h>
#include <XnFPSCalculator.h>
#include <stdio.h>
	#include <iostream>
#include "cv.h"
#include "highgui.h"
#include <boost/filesystem.hpp>

#define CHECK_RC(rc, what)											\
	if (rc != XN_STATUS_OK)											\
{																\
	printf("%s failed: %s\n", what, xnGetStatusString(rc));		\
	return rc;													\
}

#define SAMPLE_XML_PATH "SamplesConfig.xml"
using namespace xn;

//IplImage *img = 0;
//IplImage *img2 = 0;
DepthGenerator depth;
ImageGenerator image;
DepthMetaData depthMD;
ImageMetaData imageMD;
Context context;
XnFPSData xnFPS;

IplImage* rgb_data=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
IplImage* depth_data=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
IplImage* img=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
IplImage* img2=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);

std::string saveFolderName = "data";
void compareXYZRGB();
typedef union
{
  struct /*anonymous*/
  {
    unsigned char Blue; // Blue channel
    unsigned char Green; // Green channel
    unsigned char Red; // Red channel
    unsigned char Alpha; // Alpha channel
  };
  float float_value;
  long long_value;
} RGBValue;



XnUInt64 no_sample_value, shadow_value;
XnUInt64 rgb_focal_length_VGA;
XnStatus connectKinect(){
	//Connect to kinect
	printf("Connecting to Kinect. \n");	
	XnStatus nRetVal = XN_STATUS_OK;
	EnumerationErrors errors;
	nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
	if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return (nRetVal);
	}
	else if (nRetVal != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(nRetVal));
		return (nRetVal);
	}

	nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
	CHECK_RC(nRetVal, "Find depth generator");

	nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
	CHECK_RC(nRetVal, "Find image generator");

	nRetVal = xnFPSInit(&xnFPS, 180);
	CHECK_RC(nRetVal, "FPS Init");


	depth.GetAlternativeViewPointCap().SetViewPoint(image);
  if (depth.GetIntProperty ("ShadowValue", shadow_value) != XN_STATUS_OK)
      printf("[OpenNIDriver] Could not read shadow value!");

    if (depth.GetIntProperty ("NoSampleValue", no_sample_value) != XN_STATUS_OK)
      printf("[OpenNIDriver] Could not read no sample value!");

    if (image.GetIntProperty ("ZPD", rgb_focal_length_VGA) != XN_STATUS_OK)
      printf("[OpenNIDriver] Could not read virtual plane distance!");
std::cout << rgb_focal_length_VGA << std::endl;


	return nRetVal;
}


void captureRGB(const XnRGB24Pixel* pImageMap, IplImage* tmp_img = 0, bool doSave = false){

	// Convert to IplImage 24 bit, 3 channels
	for(int i = 0; i < imageMD.XRes()*imageMD.YRes();i++)
	{
		tmp_img->imageData[3*i+0]=pImageMap[i].nBlue;
		tmp_img->imageData[3*i+1]=pImageMap[i].nGreen;
		tmp_img->imageData[3*i+2]=pImageMap[i].nRed;
	}
	if (doSave){
		char buf[256];
		sprintf(buf, "data/frame%d_rgb.bmp",depthMD.FrameID());
		cvSaveImage(buf, tmp_img);
	}
}


float bad_point = std::numeric_limits<float>::quiet_NaN ();
pcl::PointCloud<pcl::PointXYZRGB> cloud_save;
void captureDepth(const XnRGB24Pixel* pImageMap, const XnDepthPixel* pDepthMap, IplImage* tmp_depth = 0, bool doSaveAsImage=false, bool doSaveAsPointCloud = false)
{
//TODO No need to do this everytime
	cloud_save.width = depthMD.XRes();
	cloud_save.height = depthMD.YRes();
	cloud_save.points.resize (depthMD.XRes() * depthMD.YRes());


	// Save only the Z value per pixel as an image for quick visualization of depth
	for(int i = 0; i < imageMD.XRes()*imageMD.YRes();i++)
	{
		short value = pDepthMap[i]/16;
		char value_pt1 = pDepthMap[i]>>8;
		char value_pt2 = pDepthMap[i]&0xFF;
		tmp_depth->imageData[3*i+0]=(char)value_pt1;
		tmp_depth->imageData[3*i+1]=(char)value_pt2;
		tmp_depth->imageData[3*i+2]=(char)value;
	}
	if (doSaveAsImage){	
		char buf2[256];
		sprintf(buf2,"data/frame%d_depth.bmp",depthMD.FrameID());
		cvSaveImage(buf2, tmp_depth);
	}
	float constant = 0.001 / rgb_focal_length_VGA; 	

}

void captureRGBAndDepth(){
	//Capture one image and compare
	XnStatus nRetVal = context.WaitAndUpdateAll();
	depth.GetMetaData(depthMD);
	image.GetMetaData(imageMD);
	const XnDepthPixel* pDepthMap = depthMD.Data();
	const XnRGB24Pixel* pImageMap = image.GetRGB24ImageMap();
	captureRGB(pImageMap, rgb_data);
	captureDepth(pImageMap, pDepthMap,depth_data);
}

void compareMouseHandler(int event, int x, int y, int flags, void *param) {
	switch (event) {
		/* left button down */
		case CV_EVENT_LBUTTONDOWN:
			cvShowImage("Comparison", rgb_data);
			break;
		case CV_EVENT_RBUTTONDOWN:
			cvShowImage("Comparison", depth_data);
			break;
		case CV_EVENT_MBUTTONDOWN:
			captureRGBAndDepth();	
	}
}

int main(int argc, char** argv) {

  RGBValue color;
	std::cout << (size_t)&color.Blue << std::endl;
	std::cout << (size_t)&color.float_value << std::endl;

	printf("Start\n");
	if (strcmp(argv[1], "--compare") == 0){
		printf("Comparing. \n");
		connectKinect();	

		IplImage * penguin =cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
		cvNamedWindow("Comparison", CV_WINDOW_AUTOSIZE);
		cvShowImage("Comparison", penguin);
		cvSetMouseCallback("Comparison", compareMouseHandler, NULL );
		cvWaitKey(0);
	}
	else if (strcmp(argv[1], "--save") == 0){
		boost::filesystem::create_directories(saveFolderName);		
		connectKinect();	
		XnStatus nRetVal = XN_STATUS_OK;

		IplImage* save_rgb =cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
		IplImage* save_visualization_depth=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);

		while (!xnOSWasKeyboardHit())
		{
			xnFPSMarkFrame(&xnFPS);
			nRetVal = context.WaitAndUpdateAll();
			depth.GetMetaData(depthMD);
			image.GetMetaData(imageMD);
			const XnDepthPixel* pDepthMap = depthMD.Data();
			const XnRGB24Pixel* pImageMap = image.GetRGB24ImageMap();
			captureRGB(pImageMap, save_rgb,true);
			captureDepth(pImageMap, pDepthMap,save_visualization_depth,true,true);
//			usleep(100000);
			printf("Test: Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));

		}
	}
	else if (strcmp(argv[1], "--annotate") == 0){
		printf("Annotate \n");

	}




	context.Shutdown();
	cvWaitKey(0);
	cvDestroyWindow("image");
	cvDestroyWindow("image2");
	cvReleaseImage(&img);
	cvReleaseImage(&img2);
	return 0;
}

